
#include "serialization/asn1.h"
#include "node_coreio.h"

#include "Debug.h"

int router_queue_push_inf(t1_inf_t* inf, uint16_t len);

static int node_coreio_hwiocommand_setregisters(void) {
	LOG_I("coreio");
    asn1_builder_t hwio_ack;
    ASN1(hwio_ack, ->TAG(PAYLOAD_RESPONSE)
                       ->TAG(HWIO_RESPONSE)
                       ->TAG(HWIO_COMMAND_ACK)
                       ->VALUE({0x00})
                       ->CLOSEALL());

    t1_inf_t inf = {.header = {.destination = NODE_SAM,
                               .source = NODE_COREIO,
                               .reply = NODE_NULL,
                               .secure_channel = 0,
                               .RFU1 = 0,
                               .RFU2 = 0},
                    .payload = hwio_ack.payload};
	router_queue_push_inf(&inf, hwio_ack.dataPtr);
	return 0;
}

typedef enum { hwioCommandSetIoRegisters = 0, TOTAL_COMMANDS } coreioModuleCommandId_t;

typedef int (*command_handler)(void);

typedef struct {
    coreioModuleCommandId_t coreioModuleCommandId;
    command_handler handler;
} coreioModuleCommands_t;

coreioModuleCommands_t const coreioModuleCommands[] = {
    [hwioCommandSetIoRegisters] = {0x00, node_coreio_hwiocommand_setregisters}};

int node_coreio_handler(t1_inf_t *inf, /*t1_len_t*/ uint16_t len) {
    asn1_tlv_t command = asn1_unwrap(inf->payload);

    if (command.tag.tagid == hwioCommand) {
        asn1_tlv_t coreioCommand = asn1_unwrap(command.value);
        for (coreioModuleCommandId_t i = 0; i < TOTAL_COMMANDS; i++) {
            if (coreioModuleCommands[i].coreioModuleCommandId == coreioCommand.tag.tagid) {
                return coreioModuleCommands[i].handler();
            }
        }
        LOG_I("No handler for command %02X", coreioCommand.tag.tagid);
    }
	return 2;
}
